/*************************************************************
SBC Example PLC ANSI-C Source Code
File: sbcplc.c
*************************************************************/
#include <math.h>
//#include "sbc.h"
#include "sbcplc.h"
/* Local Function Prototypes */
float CrossCorrelation(short *x, short *y);
int PatternMatch(short *y);
float AmplitudeMatch(short *y, short bestmatch);
/* Raised COSine table for OLA */
float rcos[OLAL] = {0.99148655f,0.96623611f,0.92510857f,0.86950446f,
		    0.80131732f,0.72286918f,0.63683150f,0.54613418f,
		    0.45386582f,0.36316850f,0.27713082f,0.19868268f,
		    0.13049554f,0.07489143f,0.03376389f,0.00851345f};

/*****************************************************************************
 * Function: InitPLC() *
 * Purpose: Perform PLC initialization of memory vectors. *
 * Inputs: *plc_state - pointer to PLC state memory *
 * Outputs: *plc_state - initialized memory. *
 * Date: 03-18-2009
 *****************************************************************************/
void InitPLC(struct PLC_State *plc_state)
{
	int i;
	plc_state->nbf=0;
	plc_state->bestlag=0;
	for (i=0;i<LHIST+SBCRT;i++)
		plc_state->hist[i] = 0;
}

/***********************************************************
 * Function: PLC_bad_frame()
 *
 * Purpose: Perform bad frame processing.
 *
 * Inputs: *plc_state - pointer to PLC state memory
 * *ZIRbuf - pointer to the ZIR response of the SBC decoder
 *
 * Outputs: *out - pointer to the output samples
 *
 * Date: 03-18-2009
 ************************************************************/
void PLC_bad_frame(struct PLC_State *plc_state, short *ZIRbuf, short *out)
{
	int i;
	float val;
	float sf;
	plc_state->nbf++;
	sf=1.0f;
	i=0;
	if (plc_state->nbf==1)
	{
		/* Perform pattern matching to find where to replicate */
		plc_state->bestlag = PatternMatch(plc_state->hist);
		plc_state->bestlag += M; /* the replication begins after the template match */
		/* Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet */
		sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);

		for (i=0;i<OLAL;i++)
		{
			val = ZIRbuf[i]*rcos[i] + sf*plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-i-1];
			if (val > 32767.0) val= 32767.0;
			if (val < -32768.0) val=-32768.0;
			plc_state->hist[LHIST+i] = (short)val;
		}
		for (;i<FS;i++)
		{
			val = sf*plc_state->hist[plc_state->bestlag+i];
			if (val > 32767.0) val= 32767.0;
			if (val < -32768.0) val=-32768.0;
			plc_state->hist[LHIST+i] = (short)val;
		}
		for (;i<FS+OLAL;i++)
		{
			val = sf*plc_state->hist[plc_state->bestlag+i]*rcos[i-FS]+plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-1-i+FS];
			if (val > 32767.0) val= 32767.0;
			if (val < -32768.0) val=-32768.0;
			plc_state->hist[LHIST+i] = (short)val;
		}
		for (;i<FS+SBCRT+OLAL;i++)
			plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
	}
	else
	{
		for (;i<FS;i++)
			plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
		for (;i<FS+SBCRT+OLAL;i++)
			plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
	}
	for (i=0;i<FS;i++)
		out[i] = plc_state->hist[LHIST+i];
  /* shift the history buffer */
	for (i=0;i<LHIST+SBCRT+OLAL;i++)
		plc_state->hist[i] = plc_state->hist[i+FS];
}

/*****************************************************************************
 * Function: PLC_good_frame()
 *
 * Purpose: Perform good frame processing. Most of the time, this function
 * just updates history buffers and passes the input to the output,
 * but in the first good frame after frame loss, it must conceal the
 * received signal as it reconverges with the true output.
 *
 * Inputs: *plc_state - pointer to PLC state memory
 * *in - pointer to the input vector
 *
 * Outputs: *out - pointer to the output samples
 * Date: 03-18-2009
 *****************************************************************************/
void PLC_good_frame(struct PLC_State *plc_state, short *in, short *out)
{
	int i;
	i=0;
	if (plc_state->nbf>0)
	{
		for (i=0;i<SBCRT;i++)
			out[i] = plc_state->hist[LHIST+i];
		for (;i<SBCRT+OLAL;i++)
			out[i] = (short)(plc_state->hist[LHIST+i]*rcos[i-SBCRT] + in[i]*rcos[OLAL-1-i+SBCRT]);
	}
	for (;i<FS;i++)
		out[i] = in[i];
	/*Copy the output to the history buffer */
	for (i=0;i<FS;i++)
		plc_state->hist[LHIST+i] = out[i];
	/* shift the history buffer */
	for (i=0;i<LHIST;i++)
		plc_state->hist[i] = plc_state->hist[i+FS];
	plc_state->nbf=0;
}

/*****************************************************************************
 * Function: CrossCorrelation()
 *
 * Purpose: Compute the cross correlation according to Eq. (4) of Goodman
 * paper, except that the true correlation is used. His formula
 * seems to be incorrect.
 *
 * Inputs: *x - pointer to x input vector
 * *y - pointer to y input vector
 *
 * Outputs: Cn - return value containing the cross-correlation of x and y
 *
 * Date: 03-18-2009
 *****************************************************************************/
float CrossCorrelation(short *x, short *y)
{
	int m;
	float num;
	float den;
	float Cn;
	float x2, y2;
	num=0;
	den=0;
	x2=0.0;
	y2=0.0;
	for (m=0;m<M;m++)
	{
		num+=((float)x[m])*y[m];
		x2+=((float)x[m])*x[m];
		y2+=((float)y[m])*y[m];
	}
	den = (float)sqrt(x2*y2);
	Cn = num/den;
	return(Cn);
}

/*****************************************************************************
 * Function: PatternMatch()
 *
 * Purpose: Perform pattern matching to find the match of template with the
 * history buffer according to Section B of Goodman paper.
 *
 * Inputs: *y : pointer to history buffer
 *
 * Outputs: return(int): the lag corresponding to the best match. The lag is
 * with respect to the beginning of the history buffer.
 *
 * Date: 03-18-2009
 *****************************************************************************/
int PatternMatch(short *y)
{
	int n;
	float maxCn;
	float Cn;
	int bestmatch;
	maxCn=-999999.0; /* large negative number */
	bestmatch=0;
	for (n=0;n<N;n++)
	{
		Cn = CrossCorrelation(&y[LHIST-M] /* x */, &y[n]);
		if (Cn>maxCn)
		{
			bestmatch=n;
			maxCn = Cn;
		}
	}
	return(bestmatch);
}

/*****************************************************************************
 * Function: AmplitudeMatch()
 *
 * Purpose: Perform amplitude matching using mean-absolute-value according
 * to Goodman paper.
 *
 * Inputs: *y : pointer to history buffer
 * bestmatch : value of the lag to the best match
 *
 * Outputs: return(float): scale factor
 *
 * Date: 03-19-2009
 *****************************************************************************/
float AmplitudeMatch(short *y, short bestmatch)
{
	int i;
	float sumx;
	float sumy;
	float sf;
	sumx = 0.0;
	sumy = 0.000001f;
	for (i=0;i<FS;i++)
	{
		sumx += abs(y[LHIST-FS+i]);
		sumy += abs(y[bestmatch+i]);
	}
	sf = sumx/sumy;
	/* This is not in the paper, but limit the scaling factor to something
	   reasonable to avoid creating artifacts */
	if (sf<0.75f) sf=0.75f;
	if (sf>1.2f) sf=1.2f;
	return(sf);
}
